//hfptlong = half long of the length of the pointer
//ptlong   = the length of the pointer
//x86-64
//#include <iostream>

#define _hfptlong  unsigned int
#define _ptlong    unsigned long
#define BYTE_SIZE  8
//#define BITMAP_V   5
#define LEVEL      27
#define BITMAP_STACK_V  1000
#define NODE_STACK_V    1000

#define NULLADRBUDDY(x) (decltype(x)(~0))
#define _DEFINE_STACKADDR(name,size) struct name {static _ptlong stackAddr[size];};_ptlong name::stackAddr[] = {0};
#define _stackAddr(T)   ((_ptlong)(T::stackAddr))
#define _stackSize(T)   (sizeof(T::stackAddr))

namespace
{
  constexpr _ptlong Bits_offset(_ptlong num,_ptlong offset = 0){return (num == 0?(offset - 1):Bits_offset(num>>1,offset+1));}

  _ptlong _Bits_offset(_ptlong num,_ptlong offset = 0){return (num == 0?(offset - 1):Bits_offset(num>>1,offset+1));}
  _ptlong _Bits_highest_clear(_ptlong num){return (num & (~(1 << _Bits_offset(num))));}

  constexpr _ptlong LOWB(_ptlong x)  {return (((_ptlong)(~0)) >> (sizeof(_ptlong) * BYTE_SIZE - x));}
  constexpr _ptlong HIGHB(_ptlong x) {return (~(LOWB(sizeof(_ptlong) * BYTE_SIZE - x)));}

  constexpr _ptlong HIGH(_ptlong x)  {return ~(LOWB(x));}
}

#define NULL ((_ptlong)0)

namespace BuddyAlloc
{
  template<typename Tstack,typename T>
  class BaseStack;
  template<typename bitStack>
  struct adrBit;
  template<typename bitStack,typename nodeStack>
  class buddyNode;
  template<typename bitStack,typename nodeStack>
  class adrbuddyNode;
  template<typename bitmapStack,typename nodeStack>
  class Buddy;
  template<int exponent,typename _bitmapStack,typename _nodeStack>
  class BuddyList;
  
  
  template<typename Tstack,typename T>
  class BaseStack  
  {
  public:
    BaseStack():addr((_ptlong*)(_stackAddr(Tstack))),volume(_stackSize(Tstack)),releasedSpace(addr)
    {
      stackpoint = (_ptlong*)(((_ptlong)addr) + volume - sizeof(T));
    }
  
    typename T::addrType increase()
    {
      _ptlong* allocAddr =
	(releasedSpace != addr 
	 ? ((releasedSpace = (_ptlong*)(((_ptlong)releasedSpace) - sizeof(_ptlong))),(_ptlong*)(*releasedSpace))
	 : (stackpoint = (_ptlong*)(((_ptlong)stackpoint) - sizeof(T))));
      T& ret = *((T*)allocAddr);
      return (&ret);
    }
    _ptlong decrease(typename T::addrType addr)
    {
      *(releasedSpace++) = (T::_RealAddr(*addr));
      return 1;
    }

  private:
    _ptlong* addr;
    _ptlong  volume;
    _ptlong* stackpoint;
    _ptlong* releasedSpace; 
  };
  
  template<typename bitStack>
  class Bitmap
  {
  public:
    Bitmap():stackpoint(_stackAddr(bitStack) + _stackSize(bitStack)),bitstack(0),spareBits((decltype(spareBits))_stackAddr(bitStack)){}
    
    enum{bits_offset = Bits_offset(sizeof(_ptlong) * BYTE_SIZE)};
    inline adrBit<bitStack> increase()
    {

      return adrBit<bitStack>(((_ptlong)spareBits != _stackAddr(bitStack) )
			    ?(*(--spareBits))
			    :adrBit<bitStack>((((
					       (bitstack == sizeof(_ptlong) * BYTE_SIZE) 
					       ?(stackpoint -= sizeof(_ptlong),
						 bitstack = 0)
					       :0)
					      ,
					      (((((stackpoint - _stackAddr(bitStack))/sizeof(_ptlong)) << bits_offset) | bitstack)))
					     )));
  
    }
    inline int decrease(adrBit<bitStack> rel)
    {
      return (*(spareBits++) = rel,1);
    }
  private:
    _hfptlong bitstack;
    adrBit<bitStack>* spareBits;
    _ptlong stackpoint;
  };


  template<typename Tstack>
  struct adrBit
  {
    adrBit(_ptlong _addr):addr((_hfptlong)_addr){}
    adrBit(_hfptlong _addr):addr(_addr){}
    adrBit(const adrBit<Tstack>& bit):addr(bit.addr){}
    adrBit()noexcept{}
    _hfptlong addr;
    int operator*()
    {
      _ptlong offset = LOWB(Bitmap<Tstack>::bits_offset) & ((_ptlong)(addr));
      _ptlong addr   = (HIGH(Bitmap<Tstack>::bits_offset) & ((_ptlong)(addr))) >> (Bitmap<Tstack>::bits_offset);
      
      return ((*(_ptlong*)((_stackAddr(Tstack) + addr * sizeof(_ptlong))) >> offset) % 2);
    }
    int set(int setting)
    {
      _ptlong offset = LOWB(Bitmap<Tstack>::bits_offset) & ((_ptlong)(this->addr));
      _ptlong addr   = HIGH(Bitmap<Tstack>::bits_offset) & ((_ptlong)(this->addr)) >> (Bitmap<Tstack>::bits_offset);
      if(setting % 2 == 0)
	{
	  (*(_ptlong*)((_stackAddr(Tstack) + addr * sizeof(_ptlong)))) &= ~(1 << offset);
	}
      else
	{
	  (*(_ptlong*)((_stackAddr(Tstack) + addr * sizeof(_ptlong)))) |= (1 << offset);
	}
      return 1;
    }

  };

  template<typename bitStack,typename nodeStack>
  class buddyNode
  {
  public:
    typedef buddyNode<bitStack,nodeStack> _self;
    typedef adrbuddyNode<bitStack,nodeStack> addrType;
    buddyNode(adrBit<bitStack> bitaddr,_ptlong vAddr):prev(&(*this)),next(&(*this)),son(addrType(~0)),bit(bitaddr),vaddr(vAddr){}
    
    void Init(adrBit<bitStack> bitaddr,_ptlong vAddr){prev = &(*this);next = &(*this);son = addrType(~0);bit = bitaddr;vaddr = vAddr;}

    addrType prev;
    addrType next;
    addrType son;
    adrBit<bitStack> bit;
    _ptlong vaddr;
    static inline _ptlong _RealAddr(_self& node)
    {
      return (_ptlong)(&(node.prev));
    }
    addrType operator&()
    {
      return addrType((_hfptlong)((_ptlong)this - _stackAddr(nodeStack)));      
    }
    inline int InsertTo(addrType tnode)//atomic!
    {
      _self& tfriend = *(this->next);
      _self& me = *this;
      _self& node = *(tnode);
      tfriend.next = &node;
      me.prev = node.prev;
      //Don't worry. me.next.next has'nt been inserted to the new ring,so it's next won't be 'me',so if we traversal, it will never end.
      (*(node.prev)).next = &me;
      node.prev = &tfriend;
      return 1;
    }

    inline int Remove()
    {
      (*((*this).prev)).next = (*this).next;
      (*((*this).next)).prev = (*this).prev;
      (*this).next = &(*this);
      (*this).prev = &(*this);
      return 1;
    }
    
    inline int Seperate(addrType newnode,adrBit<bitStack> bitaddr,_ptlong newaddr)
    {
      (*newnode).Init(bitaddr,newaddr);
      this->InsertTo(newnode);
      return 1;
    }
    template<typename _releaseNodeStack,typename _releaseBitStack>
    inline int Combine(addrType to,_releaseNodeStack _freeA,_releaseBitStack _freeB)
    {
      if(this->vaddr < (*to).vaddr)
	this->vaddr = (*to).vaddr;
      (*to).Remove();
      _freeB((*to).bit);
      _freeA(to);
      return 1;
    }

  };

  
  template<typename bitStack,typename nodeStack>
  class adrbuddyNode
  {
    typedef adrbuddyNode<bitStack,nodeStack> _self;
  public:
    adrbuddyNode(_hfptlong _addr) noexcept:addr(_addr){}
    adrbuddyNode() noexcept:addr(0){} 
    _self& operator=(const _self& a){this->addr = a.addr;return *this;}
    _hfptlong addr;


  };

  template<typename bitStack,typename nodeStack>
  buddyNode<bitStack,nodeStack>& operator*(adrbuddyNode<bitStack,nodeStack> addr)
  {
    return (*(buddyNode<bitStack,nodeStack>*)(((_ptlong)(addr.addr)) + _stackAddr(nodeStack)));
  }

  template<typename bitStack,typename nodeStack>
  inline bool operator==(const adrbuddyNode<bitStack,nodeStack>& a,const adrbuddyNode<bitStack,nodeStack>& b){return (b.addr == a.addr);}

  template<typename bitStack,typename nodeStack>  
  inline bool operator!=(const adrbuddyNode<bitStack,nodeStack>& a,const adrbuddyNode<bitStack,nodeStack>& b){return (b.addr != a.addr);}
  

  //  _ptlong _bitmapStack[BIT_MAP_STACK_V];
  //  _ptlong _nodeStack[NODE_STACK_V];

  //template<int exponent>

  template<typename bitStack,typename nodeStack>
  class Buddy
  {
    typedef adrbuddyNode<bitStack,nodeStack> adrbuddyNode_;
  public:
    Buddy(){}
    
    void Init(Bitmap<bitStack>* _bitmap,BaseStack<nodeStack,buddyNode<bitStack,nodeStack>>* _nodealloc)
    {
      bitmap = _bitmap;
      nodealloc = _nodealloc;
      nodes = _nodealloc->increase();
      (*nodes).Init(_bitmap->increase(),(~0));
    }
    template<typename _callback>
    static inline adrbuddyNode_ locate(Buddy<bitStack,nodeStack>* _this,_ptlong start,_callback get_next)
    {
      //find first
      adrbuddyNode_ ret = _this->nodes;
      decltype(ret) begin = (*ret).next;
      while(ret != begin && (*ret).vaddr != (~0))
	{
	  if(*((*ret).bit) == 0)
	    return ret;
	  ret = (*ret).next;
	}
      //if nothing
      ret = locate(get_next(start),start+1,get_next);//Stack increase
      (*ret).Remove();
      (*ret).InsertTo(_this->nodes);
      (*ret).Seperate(_this->nodealloc->increase(),_this->bitmap->increase(),(*ret).vaddr + (1 << start >> 1));
      
      return ret;
    }

    template<typename _callback>
     inline adrbuddyNode_ shape(adrbuddyNode_ addr,_ptlong size,_callback get_next_nodes)
    {
      int i = _Bits_offset(size);
      decltype(addr) ret = addr;
      decltype(addr) pre = addr;
      for(;i >=0 ; i--)
	{
	  (*addr).Remove();
	  (*addr).Seperate(nodealloc->increase(),bitmap->increase(),(*ret).vaddr + (1 << i >> 1));
	  decltype(addr) next = (*addr).next;
	  (*addr).InsertTo(get_next_nodes(i));
	  (*pre).son = addr; //first time ,it's is itself, it doesn't matter
	  ((*addr).bit).set(1);
	  if((size >> i) % 2 == 1)
	    {
	      (*addr).son = next;
	      addr = next;
	      pre  = addr;
	      ((*addr).bit).set(1);
	    }
	  
	}
      return ret;
    }

    template<typename _callback>
    static inline adrbuddyNode_ free(Buddy<bitStack,nodeStack>* _this, adrbuddyNode_ spared,_ptlong size,_callback get_pre_nodes)
    {
      decltype(spared) it = spared;
      if((*spared).son != NULLADRBUDDY(spared))
	{
	  it = free(_this,(*spared).son,_Bits_highest_clear(size),get_pre_nodes); //Stack increase
	  (*spared).son = NULLADRBUDDY(spared);
	} 
	
      decltype(it) begin = it;
      while(begin != it)
	{
	  if((*it).vaddr == (*begin).vaddr + (_ptlong)(1 << _Bits_offset(size)) || (*it).vaddr  + (_ptlong)(1 << _Bits_offset(size)) == (*begin).vaddr)
	      {
		(*begin).Combine(it,
				 [&](decltype(it) x){return _this->nodealloc->decrease(x);},
				 [&](decltype((*it).bit) x){return _this->bitmap->decrease(x);});
		(*begin).Remove();
		(*begin).InsertTo(get_pre_nodes(_Bits_offset(size)));
		return begin;
	      }
	  it = (*it).next;
	     
	}
      (*begin).bit.set(0);
      return begin;
    }
	  	  
	  
  
    //atomic<adrbuddyNode_> nodes;
    adrbuddyNode_ nodes;
    Bitmap<bitStack>* bitmap;
    BaseStack<nodeStack,buddyNode<bitStack,nodeStack>>* nodealloc;
  };
  
  template<typename bitStack,typename nodeStack>
  struct _allocHead
  {
    _ptlong size;
    adrbuddyNode<bitStack,nodeStack> ptr;

  
    inline _allocHead<bitStack,nodeStack>& operator=(_allocHead<bitStack,nodeStack>& from)
    {
      this->size = from.size;
      this->ptr  = from.ptr;
      return from;
    }
    

  };

  
  template<int exponent,typename bitStack,typename nodeStack>
  class BuddyList
  {
  public:
    BuddyList(_ptlong memory_start,_ptlong size)
    {
      for(int i = 0 ; i < exponent ; i++)
	{
	  buddies[i].Init(&bitmap,&spareNode);
	}
      for(;size != 0;size = _Bits_highest_clear(size))
	{
	  adrbuddyNode<bitStack,nodeStack> newnode = spareNode.increase();
	  (*newnode).Init(bitmap.increase(),memory_start);
	  (*newnode).InsertTo(buddies[_Bits_offset(size)].nodes);
	}
    }
    
    typedef _allocHead<bitStack,nodeStack> allocHead;
    
    inline volatile _ptlong alloc(_ptlong size)
    {
      size += sizeof(allocHead);
      
      _ptlong offset = _Bits_offset(size);
      if(size == (1 << offset))
	{
	  adrbuddyNode<bitStack,nodeStack> ret
	    = buddies[offset].locate(&(buddies[offset]),offset,[&](_ptlong _offset){return &(buddies[_offset + 1]);});
	  allocHead head = {size,ret};
	  (*ret).bit.set(1);
	  
	  *(( allocHead*)((*ret).vaddr)) = head;
	   
	  return ((*ret).vaddr + sizeof(allocHead));

	}
      else
	{
	  adrbuddyNode<bitStack,nodeStack> ret
	    = buddies[offset].locate(&(buddies[offset + 1]),offset+1,[&](_ptlong _offset){return &(buddies[_offset + 1]);});
	  allocHead head = {size,ret};
	  buddies[offset].shape(ret,size,[&](_ptlong offset){return buddies[offset - 1].nodes;});

	  *((allocHead*)((*ret).vaddr)) = head;
	  
	  return ((*ret).vaddr + sizeof(allocHead));	  
	}
    }

    inline int free(_ptlong vaddr)
    {
      allocHead head = *((allocHead*)(vaddr - sizeof(allocHead)));
      buddies[0].free(&(buddies[0]),head.ptr,head.size,[&](_ptlong offset){return buddies[offset + 1].nodes;});
      return 1;
    }
    

  private:    
    //    BaseStack<Bitmap<bitmapStack>> bitmapStack();
    Bitmap<bitStack> bitmap;
    BaseStack<nodeStack,buddyNode<bitStack,nodeStack>> spareNode;
    //    Buddy<_bitmapStack,_nodeStack> buddies[exponent]((Bitmap<_bitmapStack>*)(&bitmap),(BaseStack<_nodeStack,buddyNode<_bitmapStack,_nodeStack>>*)(&nodes));
    Buddy<bitStack,nodeStack> buddies[exponent];
  };

  _DEFINE_STACKADDR(_BIT_MAP_,BITMAP_STACK_V);
  _DEFINE_STACKADDR(_NODES_,NODE_STACK_V);
 
  #define MEMORY_SIZE 0x1000000
  int MEMORY_POOL[MEMORY_SIZE / sizeof(int)];

  BuddyList<LEVEL,_BIT_MAP_,_NODES_> memoryPool((_ptlong)MEMORY_POOL,MEMORY_SIZE);;
    
}
    
using namespace BuddyAlloc;


_ptlong Kmalloc(_ptlong size)
{
  return memoryPool.alloc(size);
}

_ptlong Kfree(_ptlong vaddr)
{
  return memoryPool.free(vaddr);
}

#include <iostream>
using namespace std;

int main()
{
  for(_ptlong i = 1 ; i < MEMORY_SIZE - sizeof(_allocHead<int,int>); i++)
    {
      _ptlong addr = Kmalloc(i);
      cout << addr<< " : " << i << endl;
      Kfree(addr);
    }
}
